`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2024/08/09 13:46:37
// Design Name: 
// Module Name: loop_adjust
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////
`include "foc_header.vh"

module loop_adjust(
    input                   sys_clk_i           ,
    input                   sys_rst_n_i         ,

    input                   loop_adjust_en_i    ,
    output                  loop_adjust_ack_o   ,

    input [5:0]             foc_mode_i          ,       //FOC运行模式
    input signed[31:0]      Open_Uq_i           ,       //开环模式下Uq参数输入
    input signed[31:0]      Open_Ud_i           ,       //开环模式下Ud参数输入
    //三环控制参数
    input signed[31:0]      Torque_Iq_PID_P_i   ,
    input signed[31:0]      Torque_Iq_PID_I_i   ,
    input signed[31:0]      Torque_Id_PID_P_i   ,
    input signed[31:0]      Torque_Id_PID_I_i   ,
    input signed[31:0]      Speed_PID_P_i       ,
    input signed[31:0]      Speed_PID_I_i       ,
    input signed[31:0]      Position_PID_P_i    ,
    input signed[31:0]      Position_PID_I_i    ,


    input signed[31:0]      desired_Iq_i        ,
    input signed[31:0]      desired_Id_i        ,
    input signed[31:0]      desired_speed_i     ,
    input signed[31:0]      desired_position_i  ,
    input signed[31:0]      actual_Iq_i         ,
    input signed[31:0]      actual_Id_i         ,
    input signed[31:0]      actual_speed_i      ,
    input signed[31:0]      actual_position_i   ,

    output reg signed[31:0] uq_o                ,
    output reg signed[31:0] ud_o
);

localparam  S_IDLE              = 'd0;
localparam  S_OPEN_LOOP         = 'd1;
localparam  S_TORQUE_LOOP       = 'd2;
localparam  S_SPEED_LOOP        = 'd3;
localparam  S_POSITION_LOOP     = 'd4;
localparam  S_LIMIT             = 'd5;
localparam  S_ACK               = 'd6;


reg[4:0] state , next_state;


wire                torque_pid_en;
wire                torque_pid_ack;
wire signed[31:0]   torque_uq ;
wire signed[31:0]   torque_ud ;
reg  signed[31:0]   desired_torque_Iq_sel;
reg  signed[31:0]   desired_torque_Id_sel;


wire                speed_pid_en;
wire                speed_pid_ack;
wire signed[31:0]   speed_fb;
reg  signed[31:0]   desired_speed_sel;

wire                position_pid_en;
wire                position_pid_ack;
wire signed[31:0]   position_fb;

reg[2:0]            position_speed_cnt;


reg signed[31:0]    uq_d0;
reg signed[31:0]    ud_d0;


assign torque_pid_en  = ( state == S_TORQUE_LOOP ) ? 1'b1 : 1'b0;
assign speed_pid_en   = ( state == S_SPEED_LOOP && position_speed_cnt == `FOC_SPEED_POSTION_CYC) ? 1'b1 : 1'b0;
assign position_pid_en = ( state == S_POSITION_LOOP && position_speed_cnt == `FOC_SPEED_POSTION_CYC) ? 1'b1 : 1'b0;


assign loop_adjust_ack_o = ( state == S_ACK ) ? 1'b1 : 1'b0;

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        state <= S_IDLE;
    else
        state <= next_state;
end

always@(*) begin
    case(state)
    S_IDLE:
        if( loop_adjust_en_i == 1'b1 && foc_mode_i == `FOC_MODE_OPEN )
            next_state <= S_OPEN_LOOP;
        else if( loop_adjust_en_i == 1'b1 && foc_mode_i == `FOC_MODE_SELFTEST )
            next_state <= S_OPEN_LOOP;
        else if( loop_adjust_en_i == 1'b1 && foc_mode_i == `FOC_MODE_TORQUE )
            next_state <= S_TORQUE_LOOP;
        else if( loop_adjust_en_i == 1'b1 && foc_mode_i == `FOC_MODE_SPEED )
            next_state <= S_SPEED_LOOP;
        else if( loop_adjust_en_i == 1'b1 && foc_mode_i == `FOC_MODE_POSTION )
            next_state <= S_POSITION_LOOP;
        else
            next_state <= S_IDLE;
    S_OPEN_LOOP:
        next_state <= S_LIMIT;
    S_TORQUE_LOOP:
        if( torque_pid_ack == 1'b1 )
            next_state <= S_LIMIT;
        else
            next_state <= S_TORQUE_LOOP;
    S_SPEED_LOOP:
        if( position_speed_cnt == `FOC_SPEED_POSTION_CYC && speed_pid_ack == 1'b1 )
            next_state <= S_TORQUE_LOOP;
        else if( position_speed_cnt == `FOC_SPEED_POSTION_CYC )
            next_state <= S_SPEED_LOOP;
        else
            next_state <= S_TORQUE_LOOP;
    S_POSITION_LOOP:
        if( position_speed_cnt == `FOC_SPEED_POSTION_CYC && position_pid_ack == 1'b1 )
            next_state <= S_SPEED_LOOP;
        else if( position_speed_cnt == `FOC_SPEED_POSTION_CYC )
            next_state <= S_POSITION_LOOP;
        else
            next_state <= S_SPEED_LOOP;
    S_LIMIT:
        next_state <= S_ACK;
    S_ACK:  
        next_state <= S_IDLE;
    default: next_state <= S_IDLE;
    endcase
end


//选择电流环的期望值
always @( posedge sys_clk_i ) begin
    if( foc_mode_i == `FOC_MODE_TORQUE ) begin
        desired_torque_Iq_sel <= desired_Iq_i;
        desired_torque_Id_sel <= desired_Id_i;
    end
    else begin
        desired_torque_Iq_sel <= speed_fb;
        desired_torque_Id_sel <= desired_Id_i;
    end
end

//选择速度环的期望值
always@( posedge sys_clk_i ) begin
    if( foc_mode_i == `FOC_MODE_SPEED )
        desired_speed_sel <= desired_speed_i;
    // else if( position_pid_en == 1'b1 && position_pid_ack == 1'b1 )
    //     desired_speed_sel <= position_fb;
    else
        desired_speed_sel <= position_fb;

end

always@( posedge sys_clk_i  or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        position_speed_cnt <= 'd0;
    else if( state == S_IDLE &&  loop_adjust_en_i == 1'b1 && (foc_mode_i == `FOC_MODE_SPEED || foc_mode_i == `FOC_MODE_POSTION))
        if( position_speed_cnt == `FOC_SPEED_POSTION_CYC )
            position_speed_cnt <= position_speed_cnt;
        else
            position_speed_cnt <= position_speed_cnt + 1'b1;
    else if( state == S_ACK && position_speed_cnt == `FOC_SPEED_POSTION_CYC )
        position_speed_cnt <= 'd0;
    else
        position_speed_cnt <= position_speed_cnt;
end


always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 ) begin
        uq_d0 <= 'd0;
        ud_d0 <= 'd0;
    end
    else if( state == S_OPEN_LOOP ) begin
        uq_d0 <= Open_Uq_i;
        ud_d0 <= Open_Ud_i;
    end
    else if( state == S_TORQUE_LOOP && torque_pid_ack == 1'b1) begin
        uq_d0 <= torque_uq;
        ud_d0 <= torque_ud;
    end
    else begin
        uq_d0 <= uq_d0;
        ud_d0 <= ud_d0;
    end
end


always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        uq_o <= 'd0;
    else if( state == S_LIMIT )
        if( uq_d0 > `UQ_MAX )
            uq_o <= `UQ_MAX;
        else if( uq_d0 < `UQ_MIN )
            uq_o <= `UQ_MIN;
        else
            uq_o <= uq_d0;
    else
        uq_o <= uq_o;
end
always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        ud_o <= 'd0;
    else if( state == S_LIMIT )
        if( ud_d0 > `UD_MAX )
            ud_o <= `UD_MAX;
        else if( ud_d0 < `UD_MIN )
            ud_o <= `UD_MIN;
        else
            ud_o <= ud_d0;
    else
        ud_o <= ud_o;
end

ila_0 ila_01(
.clk    (   sys_clk_i               ),

.probe0 (    position_fb                                                ),  //16
.probe1 (    desired_speed_sel                                   ),      //16
.probe2 (    position_pid_en                                   ),  //16
.probe3 (    Position_PID_I_i                            ),                    //6
.probe4 (    Position_PID_P_i                           )

);

position_pid#(
    .INTEGRAL_VALUE_MAX     ( `I_PID_INTEGRAL_MAX           ) ,  //积分的最大值
    .INTEGRAL_VALUE_MIN     ( `I_PID_INTEGRAL_MIN           ) ,  //积分的最小值
    .FEEDBACK_VALUE_MAX     ( `I_MAX                        ) ,  //反馈的最大值
    .FEEDBACK_VALUE_MIN     ( `I_MIN                        )    //反馈的最小值
)position_pid_torque_uq(
    .sys_clk_i              (   sys_clk_i                   ),
    .sys_rst_n_i            (   sys_rst_n_i                 ),
    
    .position_pid_en_i      (   torque_pid_en               ),
    .position_pid_ack_o     (   torque_pid_ack              ),

    .P_i                    (   Torque_Iq_PID_P_i           ),
    .I_i                    (   Torque_Iq_PID_I_i           ),
    .D_i                    (   32'd0                       ),

    .desired_value_i        (   desired_torque_Iq_sel       ),
    .actual_value_i         (   actual_Iq_i                 ),

    .feedback_value_o       (   torque_uq                   )
);
position_pid#(
    .INTEGRAL_VALUE_MAX     ( `I_PID_INTEGRAL_MAX           ),  //积分的最大值
    .INTEGRAL_VALUE_MIN     ( `I_PID_INTEGRAL_MIN           ),  //积分的最小值
    .FEEDBACK_VALUE_MAX     ( `I_MAX                        ),  //反馈的最大值
    .FEEDBACK_VALUE_MIN     ( `I_MIN                        )   //反馈的最小值
)position_pid_torque_ud(
    .sys_clk_i              (   sys_clk_i                   ),
    .sys_rst_n_i            (   sys_rst_n_i                 ),
    
    .position_pid_en_i      (   torque_pid_en               ),
    .position_pid_ack_o     (                               ),

    .P_i                    (   Torque_Id_PID_P_i           ),
    .I_i                    (   Torque_Id_PID_I_i           ),
    .D_i                    (   32'd0                       ),

    .desired_value_i        (   desired_torque_Id_sel       ),
    .actual_value_i         (   actual_Id_i                 ),

    .feedback_value_o       (   torque_ud                   )
);

position_pid#(
    .INTEGRAL_VALUE_MAX     ( `SPEED_PID_INTEGRAL_MAX       ),  //积分的最大值
    .INTEGRAL_VALUE_MIN     ( `SPEED_PID_INTEGRAL_MIN       ),  //积分的最小值
    .FEEDBACK_VALUE_MAX     ( `PID_SPEED_MAX                ),  //反馈的最大值
    .FEEDBACK_VALUE_MIN     ( `PID_SPEED_MIN                )   //反馈的最小值
)position_pid_speed(
    .sys_clk_i              (   sys_clk_i                   ),
    .sys_rst_n_i            (   sys_rst_n_i                 ),
    
    .position_pid_en_i      (   speed_pid_en                ),
    .position_pid_ack_o     (   speed_pid_ack               ),

    .P_i                    (   Speed_PID_P_i               ),
    .I_i                    (   Speed_PID_I_i               ),
    .D_i                    (   32'd0                       ),

    .desired_value_i        (   desired_speed_sel           ),
    .actual_value_i         (   actual_speed_i              ),

    .feedback_value_o       (   speed_fb                    )
);

position_pid#(
    .INTEGRAL_VALUE_MAX     ( `POSITION_PID_INTEGRAL_MAX    ),  //积分的最大值
    .INTEGRAL_VALUE_MIN     ( `POSITION_PID_INTEGRAL_MIN    ),  //积分的最小值
    .FEEDBACK_VALUE_MAX     ( `PID_POSITION_MAX             ),  //反馈的最大值
    .FEEDBACK_VALUE_MIN     ( `PID_POSITION_MIN             )   //反馈的最小值
)position_pid_position(
    .sys_clk_i              (   sys_clk_i                   ),
    .sys_rst_n_i            (   sys_rst_n_i                 ),
    
    .position_pid_en_i      (   position_pid_en             ),
    .position_pid_ack_o     (   position_pid_ack            ),

    .P_i                    (   Position_PID_P_i            ),
    .I_i                    (   Position_PID_I_i            ),
    .D_i                    (   32'd0                       ),

    .desired_value_i        (   desired_position_i          ),
    .actual_value_i         (   actual_position_i           ),

    .feedback_value_o       (   position_fb                 )
);

endmodule
